/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file RoombaTranThread.cxx
	\brief class implemenation for RoombaTranThread

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "roombaTranThread.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
  /*<-End Global Variables-->*/

RoombaTranThread::RoombaTranThread(String name) : ThreadedPipe(name)
{
  roomba_state = UNKNOWN;
	info_requested = NOT_REQUESTED;
  last_status_request.stamp();
  last_state_change.stamp();
	buf_ptr = 0;
	num_retries = 0;
}

void RoombaTranThread::initialize() {
	switch (roomba_state) {
		case UNKNOWN:
			cout << getName() << ":initialize - state UNKNOWN" << endl;
			fifo_msg.data[0] = SCI_START;
			fifo_msg.size = 1;
			writeFifo(OUTBOARD_TX);
			roomba_state = NOT_COMMANDABLE;
		case NOT_COMMANDABLE:
			cout << getName() << ":initialize - state NOT_COMMANDABLE" << endl;
			fifo_msg.data[0] = SCI_CONTROL;
			fifo_msg.data[1] = SCI_FULL;
			fifo_msg.size = 2;
			writeFifo(OUTBOARD_TX);

			num_retries = 0;
			requestSensorInfo(0);

			roomba_state = WAITING_REPLY;
			break;
		case WAITING_REPLY:
			cout << getName() << ":initialize - state WAITING_REPLY " << 6 - (int)num_retries << endl;
			if(received_info) {
				if(!(sensor_packet.group_1.bumps_wheeldrops)) {
					roomba_state = READY_FOR_COMMAND;
					cout << getName() << ":initialize - state READY_FOR_COMMAND" << endl;
				} else {
					num_retries = 0;
					requestSensorInfo(0);
					roomba_state = CLIFF_SENSOR;
					break;
				}
			}

			currentStamp.stamp();
			if((currentStamp - last_status_request ) > (STATUS_REQUEST_PERIOD * SEC2MICRO)) {
				if(num_retries > 5) roomba_state = UNKNOWN;

				requestSensorInfo(0);
				num_retries++;
			}

			break;
		case READY_FOR_COMMAND:
			break;
		case CLIFF_SENSOR:
			cout << getName() << ":initialize - state CLIFF_SENSOR " << 6 - (int)num_retries << endl;
			if(received_info) {
				if(!(sensor_packet.group_1.bumps_wheeldrops)) {
					roomba_state = NOT_COMMANDABLE;
				} 
			}

			currentStamp.stamp();
			if((currentStamp - last_status_request ) > (STATUS_REQUEST_PERIOD * SEC2MICRO)) {
				if(num_retries > 5) roomba_state = UNKNOWN;

				requestSensorInfo(0);
				num_retries++;
			}
			break;
	}
}

void RoombaTranThread::requestSensorInfo(uint8_t type)
{
	if(type > 3) return;
	info_requested = type;
	last_status_request.stamp();
	received_info = false;

	fifo_msg.data[0] = SCI_SENSORS;
	fifo_msg.data[1] = type;

	fifo_msg.size = 2;
	writeFifo(OUTBOARD_TX);
}

void RoombaTranThread::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	if(roomba_state != READY_FOR_COMMAND){
		currentStamp.stamp();
		if((currentStamp - last_state_change ) > (STATE_CHANGE_PERIOD * SEC2MICRO)) {
			initialize();
			last_state_change.stamp();
		}
	}

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("RoombaTranThread::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {

				bool full_packet = false; 
				if(buf_ptr + n > MAX_SCI_PACKET_SIZE) {
					buf_ptr = 0;
				} else {
					memcpy(&(buf[buf_ptr]),fifo_msg.data,n);
					buf_ptr += n;

					if(buf_ptr >= MIN_SCI_PACKET_SIZE) full_packet = true;

				}

				if(full_packet) {
					parseRoombaData(buf_ptr);
					if(received_info) {
						CMD_CMD((fifo_msg.data)) = CMD_ROOMBA_STATUS;
						CMD_NUM((fifo_msg.data)) = sizeof(SCISensorPacket_t);
						memcpy(CMD_DATAPTR(fifo_msg.data),&sensor_packet,sizeof(SCISensorPacket_t));
						CMD_CSUM(fifo_msg.data) = 0;
						CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

						fifo_msg.size = CMD_SIZE(fifo_msg.data);
						fifo_msg.port = 1000;

						Client * a_client;
						Client * me = shmPtr->clients.getClient(0);
						int i=0;
						while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_ROOMBA_STATUS,i)) != NULL)
						{
							fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
							fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
							writeFifo(INBOARD_TX);
							i++;
						}
						buf_ptr = 0;
					}
				}

      }
    }

    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      int n = 0;
      n=readFifo(INBOARD_RX);
      if(n>0) {
        Command cmd((unsigned char *)fifo_msg.data);
        if(!cmd.isValid())
          cout << getName() << "::update() - invalid command"<< endl << cmd ;
        else
					if(roomba_state == READY_FOR_COMMAND)
						parseInternalData(cmd);
      }
    }
  }

	if(roomba_state == READY_FOR_COMMAND) {
		currentStamp.stamp();
		if((currentStamp - last_status_request ) > (STATUS_REQUEST_PERIOD * SEC2MICRO)) {

			if((sensor_packet.group_1.bumps_wheeldrops)) {
				num_retries = 0;
				requestSensorInfo(0);
				roomba_state = CLIFF_SENSOR;
			}
			requestSensorInfo(0);
		}
	}
}

void RoombaTranThread::parseRoombaData(int n) {

	//FIXME -- no way to check for packet continutity

	uint8_t * pointer, temp;

	switch (info_requested) {
		case 0:
			if(n != 26) return;
			memcpy(&sensor_packet,buf,n);
			//fix endianness
			pointer = (uint8_t *)(&sensor_packet.group_2.angle);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_2.distance);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.voltage);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.current);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.charge);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.capacity);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			break;
		case 1:
			if(n != 10) return;
			memcpy(&(sensor_packet.group_1),buf,n);
			break;
		case 2:
			if(n != 6) return;
			memcpy(&(sensor_packet.group_2),buf,n);
			pointer = (uint8_t *)(&sensor_packet.group_2.angle);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_2.distance);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			break;
		case 3:
			if(n != 10) return;
			memcpy(&(sensor_packet.group_3),buf,n);
			pointer = (uint8_t *)(&sensor_packet.group_3.voltage);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.current);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.charge);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			pointer = (uint8_t *)(&sensor_packet.group_3.capacity);
			temp = pointer[0]; pointer[0] = pointer[1]; pointer[1] = temp;
			break;
		default:
			return;
			break;
	}

	clock_gettime(CLOCK_MONOTONIC, &t);
	sensor_packet.time = (float)t.tv_sec + (float)t.tv_nsec*1e-9;
	received_info = true;
}

void RoombaTranThread::parseInternalData(const Command &cmd)
{
  char * dataPtr = (char *)(fifo_msg.data + CMD_HEADER_SIZE);
  int radius;

  switch(cmd.cmd() ) {

    case CMD_AP_RATES: {
      APRates_t * vel_rate =  (APRates_t *)dataPtr;

      vel_rate->velocity = M_TO_LON(vel_rate->velocity,5.5); //FIXME - HACK
      vel_rate->velocity *= 1000;//velocity in mm/s
      if(vel_rate->velocity < 20) vel_rate->velocity = 0.0;
      if(fabs(vel_rate->turn_rate) < 0.02) (vel_rate->turn_rate) = 0.0;

      // Convert the velocity, turn_rate into roomba serial commands
      if( vel_rate -> turn_rate == 0 )
        radius = 0x8000;
      else if (vel_rate -> velocity == 0){
        vel_rate -> velocity = 100*fabs(vel_rate -> turn_rate);
        radius = (int)(-1*(vel_rate -> turn_rate) / fabs(vel_rate -> turn_rate));}
      else
        radius = (int)(-1*(vel_rate -> velocity) / (vel_rate -> turn_rate));

			drive((int)(vel_rate->velocity),radius);

      break;
      }
    case CMD_START: {
			fifo_msg.data[0] = SCI_MOTORS;
			fifo_msg.data[1] = 0x05;
			fifo_msg.size = 2;
			writeFifo(OUTBOARD_TX);
      break;
      }
    case CMD_STOP: {
			fifo_msg.data[0] = SCI_MOTORS;
			fifo_msg.data[1] = 0x00;
			fifo_msg.size = 2;
			writeFifo(OUTBOARD_TX);
      break;
      }
    default:
      break;
  }
}

void RoombaTranThread::drive(int velocity, int radius)
{
	velocity > 500 ? velocity = 500 : velocity = velocity;
	velocity < -500 ? velocity = -500 : velocity = velocity;

	if(radius != (0x8000)) {
		radius > 2000 ? radius = 2000: radius = radius;
		radius < -2000 ? radius = -2000: radius = radius;
	}

	uint8_t vhi = velocity >> 8;
	uint8_t vlo = velocity & 0xff;
	uint8_t rhi = radius >> 8;
	uint8_t rlo = radius & 0xff;

	fifo_msg.data[0] = SCI_DRIVE;
	fifo_msg.data[1] = vhi;
	fifo_msg.data[2] = vlo;
	fifo_msg.data[3] = rhi;
	fifo_msg.data[4] = rlo;

	fifo_msg.size = 5;
	writeFifo(OUTBOARD_TX);

	if(verbose) {
		cout << "Velocity command: " << (int)(velocity) << endl;
		cout << "Radius command: " << (int)(radius) << endl;
	}
}
